/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "stdafx.h"
#include "Con_Impedance.h"

namespace RobotPlant { namespace Mechanism{

	using namespace std;
	using namespace RobotPlant::Math;

	Con_Impedance::Con_Impedance(const Chain & robot, 
		const ColumnVector & Mp, const ColumnVector & Dp, const ColumnVector & Kp,
		const ColumnVector & Mo, const ColumnVector & Do, const ColumnVector & Ko)
	{
		_pc = ColumnVector(3); 
		_pc.SetZero();
		_pcp = _pc;
		_pcp_prev = _pc;
		_pcpp = _pc;
		_pcpp_prev = _pc;
		_qc = Quaternion();
		_qcp = _qc;
		_qcp_prev = _qc;
		_quat = _qc;
		_wc = _pc;
		_wcp = _pc;
		_wcp_prev = _pc;

		_Mp = Matrix(robot.Get_Dof(),robot.Get_Dof());
		_Dp = Matrix(robot.Get_Dof(),robot.Get_Dof());
		_Kp = Matrix(robot.Get_Dof(),robot.Get_Dof());
		_Mo = Matrix(robot.Get_Dof(),robot.Get_Dof());
		_Do = Matrix(robot.Get_Dof(),robot.Get_Dof());
		_Ko = Matrix(robot.Get_Dof(),robot.Get_Dof());

		_Mp.Identity();
		_Dp.Identity();
		_Kp.Identity();
		_Mo.Identity();
		_Do.Identity();
		_Ko.Identity();

		Set_Mp(Mp);
		Set_Dp(Dp);
		Set_Kp(Kp);
		Set_Mo(Mo);
		Set_Do(Do);
		Set_Ko(Ko);

		Frame3 pose = robot.ForwardKine_Pose(robot.Get_JointValues());
		_qc = Quaternion(pose.Rotation);
	}

	void Con_Impedance::Set_Mp(const ColumnVector & Mp)
	{
		if(Mp.GetCount() != 3)
		{
			throw(new exception(" Argument _Kd's count number should be the same as 3."));
		}

		for(int i=0; i<3; i++)
			_Mp(i,i) = Mp[i];
	}

	void Con_Impedance::Set_Mp(const double Mp_i, const int i)
	{
		if( (i < 0) || (i > 3) )
		{
			throw(new exception(" Argument i should be 0, 1 or 2."));
		}

		_Mp(i,i) = Mp_i;
	}

	void Con_Impedance::Set_Dp(const ColumnVector & Dp)
	{
		if(Dp.GetCount() != 3)
		{
			throw(new exception(" Argument Dp's count number should be the same as dof."));
		}

		for(int i=0; i<3; i++)
			_Dp(i,i) = Dp[i];
	}

	void Con_Impedance::Set_Dp(const double Dp_i, const int i)
	{
		if( (i < 0) || (i > 3) )
		{
			throw(new exception(" Argument i should be 0, 1 or 2."));
		}

		_Dp(i,i) = Dp_i;
	}

	void Con_Impedance::Set_Kp(const ColumnVector & Kp)
	{
		if(Kp.GetCount() != 3)
		{
			throw(new exception(" Argument Kp's count number should be the same as dof."));
		}

		for(int i=0; i<3; i++)
			_Kp(i,i) = Kp[i];
	}

	void Con_Impedance::Set_Kp(const double Kp_i, const int i)
	{
		if( (i < 0) || (i > 3) )
		{
			throw(new exception(" Argument i should be 0, 1 or 2."));
		}

		_Kp(i,i) = Kp_i;
	}

	void Con_Impedance::Set_Mo(const ColumnVector & Mo)
	{
		if(Mo.GetCount() != 3)
		{
			throw(new exception(" Argument Mo's count number should be the same as dof."));
		}

		for(int i=0; i<3; i++)
			_Mo(i,i) = Mo[i];
	}

	void Con_Impedance::Set_Mo(const double Mo_i, const int i)
	{
		if( (i < 0) || (i > 3) )
		{
			throw(new exception(" Argument i should be 0, 1 or 2."));
		}

		_Mo(i,i) = Mo_i;
	}

	void Con_Impedance::Set_Do(const ColumnVector & Do)
	{
		if(Do.GetCount() != 3)
		{
			throw(new exception(" Argument Do's count number should be the same as dof."));
		}

		for(int i=0; i< 3; i++)
			_Do(i,i) = Do[i];
	}

	void Con_Impedance::Set_Do(const double Do_i, const int i)
	{
		if( (i < 0) || (i > 3) )
		{
			throw(new exception(" Argument i should be 0, 1 or 2."));
		}

		_Do(i,i) = Do_i;
	}

	void Con_Impedance::Set_Ko(const ColumnVector & Ko)
	{
		if(Ko.GetCount() != 3)
		{
			throw(new exception(" Argument Ko's count number should be the same as 3."));
		}

		for(int i=0; i<3; i++)
			_Ko(i,i) = Ko[i];
	}

	void Con_Impedance::Set_Ko(const double Ko_i, const int i)
	{
		if( (i < 0) || (i > 3) )
		{
			throw(new exception(" Argument i should be 0, 1 or 2."));
		}

		_Ko(i,i) = Ko_i;
	}

	void Con_Impedance::Control(const ColumnVector & pdpp, const ColumnVector & pdp,
		const ColumnVector & pd, const ColumnVector & wdp,
		const ColumnVector & wd, const Quaternion & qd,
		const ColumnVector & f, const ColumnVector & n,
		const double dt)
	{
		if(pdpp.GetCount() !=3)
		{
			throw(new exception(" Argument pdpp's count number should be 3."));
		}
		if(pdp.GetCount() !=3)
		{
			throw(new exception(" Argument pdp's count number should be 3."));
		}
		if(pd.GetCount() != 3)
		{
			throw(new exception(" Argument pd's count number should be 3."));
		}
		if(wdp.GetCount() !=3)
		{
			throw(new exception(" Argument wdp's count number should be 3."));
		}
		if(wd.GetCount() !=3)
		{
			throw(new exception(" Argument wd's count number should be 3."));
		}
		if(f.GetCount() !=3)
		{
			throw(new exception(" Argument f's count number should be 3."));
		}
		if(n.GetCount() !=3)
		{
			throw(new exception(" Argument n's count number should be 3."));
		}

		static bool first=true;
		if(first)
		{
			_qc = qd;
			_qcp = _qc.Dot(_wc, 1 /*BASE_FRAME*/);
			_qcp_prev = _qcp;
			_wc = wd;
			_wcp = wdp;
			first = false;
		}
		if(_qc.DotProduct(qd) < 0)
			_quat = qd*(-1);
		else
			_quat = qd;

		_qcd = _quat * _qc.Inverse();

		// Solving _pcpp, _pcp, _pc with the translational impedance
		_pcd = _pc - pd;
		_pcdp = _pcp - pdp;
		//  _pcpp = pdpp + _Mp.i()*(f - _Dp*_pcdp - _Kp*_pcd - 2*_qcd.s()*Km.t()*_qcd.v()); // (21)
		_pcpp = pdpp + _Mp.Inverse()*(f - _Dp*_pcdp - _Kp*_pcd);
		_pcp = _pcp_prev + Integ_Trap(_pcpp, _pcpp_prev, dt);
		_pc = _pc + Integ_Trap(_pcp, _pcp_prev, dt);

		// Solving _wcp, _wc, _qc with the rotational impedance
		_wcd = _wc - wd;
		_Ko_prime = 2*_qcd.E(1 /*BASE_FRAME*/)*_Ko;                                   //(23)
		//  Km_prime = (_qcd.s()*_qcd.E(BASE_FRAME) - _qcd.v()*_qcd.v().t())*Km;   // (24)
		//  _wcp = wdp + _Mo.i()*(n - _Do*_wcd - _Ko_prime*_qcd.v() - Km_prime*_pcd); // (22)
		_wcp = wdp + _Mo.Inverse()*(n - _Do*_wcd - _Ko_prime*_qcd.V); // (22)
		_wc = _wc + Integ_Trap(_wcp, _wcp_prev, dt);
		_qcp = _qc.Dot(_wc, 1 /*BASE_FRAME*/);
		Integ_quat(_qcp, _qcp_prev, _qc, dt);
	}


}}